﻿using System;
using FRRobot;
using RestService;
using System.Threading;

/// <summary>
/// Class that know how to manage the robot FANUC
/// </summary>
public class RobotFanuc: IRobot
{
    private static RobotFanuc robot = null;
    private static FRCRobot fanuc = null;
    private String robotIP;
    private String defaultRobotIP = "150.128.87.218"; 
    private String robotVersion="04.01";
     Boolean connected;

    public static RobotFanuc getRobot() // This is to ensure that only one instace of RobotFanuc will exists, it is the "Singleton pattern"        
    {
        if (robot == null)
        {
            robot = new RobotFanuc();
        }
        return robot;
    }

	private RobotFanuc() // The constructor must be private to ensure the Singleton pattern
	{        
        fanuc = new FRCRobot();
	}

    public RobotResponse robotGeneralInfo(string type)
    {
        RobotResponse rr = new RobotResponse();
        String text;
        text= "Fanuc Robot, v." + robotVersion + Environment.NewLine +
              "Robot IP: " + robotIP + Environment.NewLine;
        if (type=="log")
        {
            text = text + Environment.NewLine +LogRecording.getMessage()+ Environment.NewLine;
        }
        rr.message = text;
        return rr;
    }

    

    public void connect()
    {
        LogRecording.LogMessage("connect('') received, using default Robot IP: " + defaultRobotIP);
        connect(defaultRobotIP);
    }

    public void connect(String ip)
    {
        LogRecording.LogMessage("connect('" + ip + "') received");
        Boolean connected;
        fanuc = new FRCRobot();
        robotIP = ip;
        if (!fanuc.IsConnected)
        {
            try
            {
                //fanuc.ConnectEx(robotIP, true, 1, 1); // IP, nowait, retries, period  
                fanuc.Connect(robotIP);
                System.Threading.Thread.Sleep(3000);
                connected = fanuc.IsConnected;
            }
            catch (Exception e)
            {
                LogRecording.LogMessage("Exception in connect('" + ip + "'): " + e.Message);
                RobotException re = new RobotException("Connection with robot failed. Using IP:" + robotIP + ". " + e.Message);
                throw (re);
            }
/*
            if (!connected)
            {
                RobotException re = new RobotException("Connection with robot failed. Using IP:" + robotIP);
                throw (re);
            }
 * */
        }
    }
   
    public Boolean isConnected()
    {
        Boolean connected;       
        try
        {            
            connected = fanuc.IsConnected;
        }
        catch (Exception e)
        {
            RobotException re = new RobotException("Could not retrieve status from the robot." + e.Message);
            throw (re);
        }
        return connected;
    }

    

    private RobotJointsValues  robotJointsToJoints(FRCJoint robotJoints)
    {
        RobotJointsValues jv = new RobotJointsValues();
        for (short i = 1; i <= 6; i++)
        {
            double doubleValue = robotJoints[i];
            int integerValue = Convert.ToInt32(doubleValue);
            jv.setJoint(i, integerValue);
        }
        return jv;
    }

    public RobotJointsValues robotCurrentJointsValues()
    {

        FRCCurPosition curPosition = fanuc.CurPosition;
        FRCCurGroupPosition curGroupPosition = curPosition.Group[1,FRECurPositionConstants.frWorldDisplayType];
        FRCJoint joint = curGroupPosition.Formats[FRETypeCodeConstants.frJoint];
        RobotJointsValues jv = robotJointsToJoints(joint);
        return jv;
    }

    public RobotXYZWPRValues robotCurrentXYZWPRValues()
    {
        RobotXYZWPRValues pos = null;

        FRCCurPosition curPosition = fanuc.CurPosition;
        FRCXyzWpr xyzWpr = curPosition.Group[1, FRECurPositionConstants.frWorldDisplayType].Formats[FRETypeCodeConstants.frExtXyzWpr];
        pos = new RobotXYZWPRValues(xyzWpr.X, xyzWpr.Y, xyzWpr.Z, xyzWpr.W, xyzWpr.P, xyzWpr.R);
        return pos;
    }

    internal RobotJointsValues robotCurrentJoints()
    {
        RobotJointsValues jv = new RobotJointsValues();
        for (int i = 0; i <= 5; i++)
        {
            int value = i*10;
            jv.setJoint(i + 1, value);
        }
        return jv;
    }

    internal String getDefaultRobotIP()
    {
        return defaultRobotIP;
    }

    public void robotMoveToJoints(RobotJointsValues jv)
    {
        try {

            FRCSysPositions sysPositions = fanuc.RegPositions;
            FRCSysPosition sysPosition = sysPositions[1];
            FRCSysGroupPosition sysGroupPosition = sysPosition.Group[1];
            FRCJoint joint = sysGroupPosition.Formats[FRETypeCodeConstants.frJoint];
            sysGroupPosition.Type = FRETypeCodeConstants.frJoint;
            for (short i = 1; i <= 6; i++)
            {
                joint[i] = jv.getJoint(i);
            }
            sysGroupPosition.Update();
            sysGroupPosition.Moveto();
        } catch (System.Runtime.InteropServices.COMException e) {
            
        }

    }

    public void robotMoveToXYZWPR(RobotXYZWPRValues xyzWprValues )
    {

        RobotXYZWPRValues  real = new RobotXYZWPRValues();
        real = robotCurrentXYZWPRValues();

        //while (distance(real,xyzWprValues)>1) {
            try
            {
                FRCSysPositions sysPositions = fanuc.RegPositions;
                FRCSysPosition sysPosition = sysPositions[1];
                FRCSysGroupPosition sysGroupPosition = sysPosition.Group[1];
                FRCXyzWpr xyzWpr = sysGroupPosition.Formats[FRETypeCodeConstants.frXyzWpr];
                xyzWpr.X = xyzWprValues.x;
                xyzWpr.Y = xyzWprValues.y;
                xyzWpr.Z = xyzWprValues.z;
                xyzWpr.W = xyzWprValues.w;
                xyzWpr.P = xyzWprValues.p;
                xyzWpr.R = xyzWprValues.r;

                sysGroupPosition.Update();
                sysGroupPosition.Moveto();
            }
            catch (System.Runtime.InteropServices.COMException e) { }
            //Thread.Sleep(500);
            real = robotCurrentXYZWPRValues();
        //}

    }

    private double distance(RobotXYZWPRValues p1, RobotXYZWPRValues p2)
    {
        double x = (double)(p1.x - p2.x);
        double y = (double)(p1.y - p2.y);
        double z = (double)(p1.z - p2.z);
        double w = (double)(p1.w - p2.w);
        double p = (double)(p1.p - p2.p);
        double r = (double)(p1.r - p2.r);

        double value= Math.Sqrt(x *x + y*y + z*z+ w*w+p*p + r*r);
        return value;
        
    }
}
